package controller

import (
	"fmt"
	"math"

	"github.com/gin-gonic/gin"
)

var LPAstar = new(lpaAstar)

type lpaAstar struct {
	Num int
}

var obs = map[[2]int]struct{}{}
var start = [2]int{}
var goal = [2]int{}
var g = map[[2]int]float64{}
var rhs = map[[2]int]float64{}
var u = map[[2]int][2]float64{}

func (ctx *lpaAstar) Base(c *gin.Context) {
	var req *Req
	c.ShouldBind(&req)
	for i := 400; i < 430; i++ {
		for j := 40; j < 540; j++ {
			obs[[2]int{i, j}] = struct{}{}
		}
	}

	if req.ChangePoint[0][0] > 0 {
		point := req.ChangePoint
		for i := point[0][0]; i < point[1][0]; i++ {
			for j := point[0][1]; j < point[1][1]; j++ {
				obs[[2]int{i, j}] = struct{}{}
				for _, s_n := range getNeighbor1([2]int{i, j}) {
					updateVertex(s_n, start, goal, g, rhs, u)
				}
			}
		}

		computeShortestPath(0, c)

	} else {
		num := 0
		// 开始点，目标点，拐弯代价，目前最小代价，u
		start = [2]int{req.Start[0], req.Start[1]}
		goal = [2]int{req.Goal[0], req.Goal[1]}
		g = map[[2]int]float64{}
		for i := 0; i < 800; i++ {
			for j := 0; j < 800; j++ {
				g[[2]int{i, j}] = math.MaxFloat64
			}
		}
		rhs = map[[2]int]float64{}
		u = map[[2]int][2]float64{}
		rhs[start] = 0
		u[start] = calculateKey(start, goal, g, rhs)
		computeShortestPath(num, c)
	}

}

func computeShortestPath(num int, c *gin.Context) {
	a, b := 0, 0
	for {
		num++
		s, _ := topKey(u)
		// if num == 19999 {
		// 	fmt.Println(num, a, b)
		// 	c.JSON(200, gin.H{
		// 		"code": 0,
		// 		"msg":  "成功",
		// 		"data": gin.H{
		// 			"lines": [][2]int{},
		// 		},
		// 	})
		// 	return
		// }
		if u[s][0] >= calculateKey(goal, goal, g, rhs)[0] && u[s][1] >= calculateKey(goal, goal, g, rhs)[1] && g[goal] == rhs[goal] {
			fmt.Println(num)
			c.JSON(200, gin.H{
				"code": 0,
				"msg":  "成功",
				"data": gin.H{
					"lines": createPath(start, goal, g),
				},
			})
			return
		}
		// fmt.Println(s)
		// time.Sleep(time.Second)
		delete(u, s)
		if g[s] > rhs[s] {
			g[s] = rhs[s]
			a++
		} else {
			b++
			g[s] = float64(99999)
			updateVertex(s, start, goal, g, rhs, u)
		}

		for _, s_n := range getNeighbor1(s) {
			updateVertex(s_n, start, goal, g, rhs, u)
		}
	}
}

func calculateKey(s, goal [2]int, g, rhs map[[2]int]float64) [2]float64 {
	return [2]float64{math.Min(g[s], rhs[s]) + h(s, goal), math.Min(g[s], rhs[s])}
}

func h(s, goal [2]int) float64 {
	return math.Sqrt(math.Pow(float64(s[0])-float64(goal[0]), 2) + math.Pow(float64(s[1])-float64(goal[1]), 2))
}

func topKey(u map[[2]int][2]float64) ([2]int, [2]float64) {
	var s [2]int
	min := 99999.0
	for i, v := range u {
		if v[0]+v[1] < min {
			min = v[0] + v[1]
			s = i
		}
	}
	return s, u[s]
}

func getNeighbor1(minItem [2]int) [][2]int {
	motions := [8][2]int{
		{-1, 0}, {-1, 1}, {0, 1}, {1, 1}, {1, 0}, {1, -1}, {0, -1}, {-1, -1},
	}
	// motions := [16][2]int{
	// 	{-1, 0}, {-2, 1}, {-1, 1}, {-1, 2}, {0, 1}, {1, 2}, {1, 1}, {2, 1}, {1, 0}, {2, -1}, {1, -1}, {1, -2}, {0, -1}, {-1, -2}, {-1, -1}, {-2, -1},
	// }
	list := [][2]int{}
	for _, u := range motions {
		if minItem[0]+u[0] > 0 && minItem[1]+u[1] > 0 && minItem[0]+u[0] < 800 && minItem[1]+u[1] < 600 {
			if !isCollision([2]int{minItem[0] + u[0], minItem[1] + u[1]}, goal, obs) {
				list = append(list, [2]int{minItem[0] + u[0], minItem[1] + u[1]})

			}
			// if minItem[0]+u[0] > 0 && minItem[1]+u[1] > 0 && !(minItem[0]+u[0] > 400 && minItem[0]+u[0] < 430 && minItem[1]+u[1] > 40 && minItem[1]+u[1] < 540) {
			// list = append(list, [2]int{minItem[0] + u[0], minItem[1] + u[1]})
		}
	}
	return list
}

func updateVertex(s, start, goal [2]int, g, rhs map[[2]int]float64, u map[[2]int][2]float64) {

	if s != start {
		rhs[s] = float64(99999)
		for _, s_n := range getNeighbor1(s) {
			if g[s_n]+cost(s_n, s) < rhs[s] {
				rhs[s] = g[s_n] + cost(s_n, s)
			}
		}
	}

	_, ok := u[s]
	if ok {
		delete(u, s)
	}
	if g[s] != rhs[s] {
		u[s] = calculateKey(s, goal, g, rhs)
	}
}

func cost(s, goal [2]int) float64 {
	if isCollision(s, goal, obs) {
		return float64(999999)
	}
	return math.Sqrt(math.Pow(float64(s[0])-float64(goal[0]), 2) + math.Pow(float64(s[1])-float64(goal[1]), 2))
}

// 求路径，竟然完全不耗性能
func createPath(start, goal [2]int, g map[[2]int]float64) [][2]int {
	path := [][2]int{}
	s := goal
	for {
		minS := [2]int{}
		minV := math.MaxFloat64
		for _, s_n := range getNeighbor1(s) {
			if !isCollision(s, s_n, obs) {
				if g[s_n] < minV {
					minV = g[s_n]
					minS = s_n
				}
			}
		}
		s = minS
		path = append(path, s)
		if s == start {
			break
		}
	}
	return path
}

func isCollision(start, end [2]int, obs map[[2]int]struct{}) bool {
	_, ok1 := obs[start]
	_, ok2 := obs[end]
	if ok1 || ok2 {
		return true
	}
	return false
}
